使用者插補範例

此範例程式說明如何使用 KINGSTAR 子系統的直接控制模式，直接控制模式能讓應用程式透過子系統使用自己的插補及傳送循環命令到裝置上，子系統接受位置、速度或扭矩直接命令，但非所有驅動器支援扭矩命令。此範例使用位置命令，速度與扭矩命令皆為相同的運作方式。

此範例亦介紹了處理 EtherCAT 通訊、配置 EtherCAT 從站和 KINGSTAR 子系統等的 KINGSTAR 現場總線函式，也介紹了配置軸和設定軸位置的 KINGSTAR Motion 函式，但不包含移動函式，若欲使用移動函式，需使用其中一個主插補模式。

編譯及執行範例程式

檔案位在：C:\Users\Public\Public Documents\IntervalZero\KINGSTAR SDK\<Version Number>\Samples\UserInterpolationSample，將 UserInterpolationSample.sln 開啟並編譯之 。

注意：檔案總管 (File Explorer) 有兩個路徑：階層路徑與完整路徑，階層路徑顯示在地址欄中；完整路徑顯示在檔案總管上方。右鍵點擊 UserInterpolationSample.sln 後點選 Properties，將可看見位置 (Location) 為 C:\Users\Public\Documents\IntervalZero\KINGSTAR SDK\<Version Number>\Samples\UserInterpolationSample，此即為完整路徑；而注意階層路徑為 Public Documents。若使用非英文的 Windows 系統，而需要複製貼上路徑至地址欄中以加快查找範例速度，則請務必使用完整路徑；若想要透過點擊瀏覽範例檔案夾，則請使用階層路徑。英文版 Windows 之檔案夾將自動重新導向，因此就算貼上階層路徑，檔案總管亦可引導至範例程式。

下圖為範例程式的輸出：

在 Visual Studio 中設定專案 properties

範例程式使用 Visual Studio 2019 的 C 語言開發，開發應用程式時，只要此應用程式為 64 位元，即可自行選擇開發環境，因控制即時子系統需使用 64 位元。

創建應用程式時需在 Visual Studio 內修改以下屬性：

1. 在 Solution Explorer 中的專案名稱上點擊右鍵，接著點選 Properties。
2. 進入 (Project name) Property Pages 對話框中的左窗格，點開 C/C++ 清單並點選 General。
3. 在右方區域的 Additional Include Directories 方框輸入 "$(RTX64SDKDir4)include;$(KINGSTARSDKDir4)include;%(AdditionalIncludeDirectories)" （無空白鑑）。
4. 在左窗格中點開 Linker 清單後點選 General。
5. 右側 Additional Library Directories 方框中，輸入"$(RTX64SDKDir4)lib\$(Rtx64Platform);$(KINGSTARSDKDir4)lib\amd64;%(AdditionalLibraryDirectories)"（無空白鍵）。
6. 在左窗格中的 Linker 清單中，點選 Input。
7. 在右側 Additional Dependencies 方框中，依照建置目標輸入以下字串：
• RtssDebug 與 RtssRelease 建置：輸入不含空白鍵的 "startupCRT.lib;libcmtd.lib;libcpmtd.lib;libucrtd.lib;libvcruntimed.lib;rtx_rtss.lib;ksapi_rtss.lib"。
• Debug 與 Release 建置：輸入不含空白鍵的 "rtapi.lib;ksapi.lib" 。
8. 點擊 Apply 後再按一下 OK。
9. 在專案的標頭檔（名稱為 ProjectName.h）之 #endif // UNDER_RTSS 底下，輸入以下代碼：

#include <ksapi.h>
#include <ksmotion.h>

分組函式

為增加編碼的可讀性，函式在範例程式中使用 #pragma 分組，透過此 #pragma 碼可知函式的使用方式。

執行循環任務

我們定義函式 CyclicTask，此函式在每個周期呼叫。

int CyclicTask(PVOID Context) {
   int TargetAxis = 0;

   // Moving at a fixed velocity of 360 degrees per second
   double Position = 0;
   GetAxisPosition(TargetAxis, mcSetValue, &Position);
   BOOL HomeAbsSwitch;
   BOOL LimitSwitchPos;
   BOOL LimitSwitchNeg;
   BOOL Simulation;
   BOOL CommunicationReady;
   BOOL ReadyForPowerOn;
   BOOL PowerOn;
   BOOL IsHomed;
   BOOL AxisWarning;
   GetAxisInfo(TargetAxis, &HomeAbsSwitch, &LimitSwitchPos, &LimitSwitchNeg, &Simulation, &CommunicationReady, &ReadyForPowerOn, &PowerOn, &IsHomed, &AxisWarning);
   if (PowerOn == TRUE) {
      Position += (double)360 / (double)1000; 
      SetAxisPosition(TargetAxis, Position);
   }
   return 0;
}

兩個宣告的變數為：TargetAxis 與 Position。

• TargetAxis 為軸索引。
• Position 為軸位置。

使用 GetAxisPosition 以取得軸的目前位置，接著使用 GetAxisInfo 來確認軸已被啟動，軸啟動後可使用 SetAxisPosition 設定軸的目標位置。

配置 KINGSTAR 子系統

開啟 KINGSTAR 子系統前，必須先初始化處理序及變數，若發生非預期錯誤，該呼叫將回傳 KsError 類型中的錯誤碼，在子系統成功初始化之前其他函式皆無法使用，所有先前的設定都將被覆寫。

在初始化處理序與變數前，我們宣告以下變數，這些變數在 int _tmain 中使用。

SlaveStatus axis = { 0 };
int axisResolution = 10000;
McPidSettings myPid = { 1, 0, 0, 0, 1, 0.003, 0.003, 0, 0.2, 0.1, FALSE, FALSE, 0, 5000 };
McProfileSettings Motion = { 3, 3600, 3600, 36000, 36000, 3600000, 0 };
KsCommandStatus Command = { 0 };
KsError Code = errNoError;

• SlaveStatus axis = { 0 } – SlaveStatus 為包含 EtherCAT 從站細節的結構，我們使用 axis 以獲取從站資訊。
• int axisResolution = 10000 – axisResolution 為軸（伺服驅動器）的解析度。初始值設為 10000，實際值依使用的軸而定。
• McPidSettings myPid – McPidSettings 為包含 KINGSTAR Motion PID 設定的結構，因此我們使用 myPid 來初始化 PID 值。
• McProfileSettings Motion – McProfileSettings 為包含與運動相關設定的結構，如加速度、減速度與加加速度，因此我們使用 Motion 來初始化運動值。
• KsCommandStatus Command = { 0 } – KsCommandStatus 為包含函式狀態的結構，可得知函式是否正在執行、完成、回傳任何錯誤及何錯誤，我們使用 Command 以獲取函式狀態。
• KsError Code = errNoError – KsError 為包含 KINGSTAR 所有錯誤碼的列舉類型，errNoError 表示函式已成功。

初始化程序包含在 #pragma region Initialization 內，使用 Code 以檢查函式是否成功執行，其將接收函式的回傳值，若 Code 非 errNoError，程式將會跳至 End 方塊並執行裡面的程式碼。

準備連接至 KINGSTAR 子系統

首先呼叫 Create，準備連接應用程式至 KINGSTAR 子系統，開始任何動作前，必須先呼叫 Create。

Code = Create(0, 0);
if (Code != errNoError) {
   RtPrintf("Failed to create: 0x%x\n", Code);
   goto End;
}

設定 EtherCAT 循環時間

SetCycleTime設定 EtherCAT 循環時間（秒），其時間單位為秒。在此範例中，cycle1000 代表 1 毫秒，若欲輸入一數字，請輸入 SetCycleTime(0.001)，我們使用變數以代表數字，可在 ksapi.h 中找到以下的定義，其位於 C:\Program Files\IntervalZero\KINGSTAR SDK\4.0\Include。

變數	數字
cycle100	0.0001
cycle125	0.000125
cycle250	0.000250
cycle500	0.0005
cycle1000	0.001

欲使用低於 1 毫秒的循環時間，須備有高速計時器套件。注意非所有軸皆支援快速循環時間，若選擇了不支援的循環時間，則每個軸的更新時間會自動且各自延長，欲使用快速循環時間，請確保電腦上的網卡可使用，只有具有低延遲的網卡才可支持快速循環。

Code = SetCycleTime(cycle1000);
if (Code != errNoError) {
   RtPrintf("Failed to set cycle time: 0x%x\n", Code);
   Destroy();
   goto End;
}

停用 RTX64 伺服主控台 (Server Console) 上的記錄

EnableServerLog啟用或停用 RTX64 Server Console 上的即時訊息，若將其停用，則主控台將只顯示 KINGSTAR 訊息，因此我們選擇將其停用。

Code = EnableServerLog(FALSE);
if (Code != errNoError) {
   RtPrintf("Failed to set server log: 0x%x\n", Code);
   Destroy();
   goto End;
}

設定存取模式

SetAxisAccessMode設定 EtherCAT 驅動器之資料傳送模式，存取模式決定軸可用的控制模式，存取模式可在 KsAccessMode 列舉類型中選擇，預設之存取模式為 accessVelPos，而我們使用 accessPosVel。

Code = SetAxisAccessMode(accessPosVel);
if (Code != errNoError) {
   RtPrintf("Failed to set access mode: 0x%x\n", Code);
   Destroy();
   goto End;
}

啟用軸之數位輸入

EnableAxisInput啟用或停用軸之數位輸入的存取，首三個輸入位元為負超程 (Overtravel)、正超程與原點復歸感測器，若輸入啟用後可使用超程位元。

Code = EnableAxisInput(TRUE);
if (Code != errNoError) {
   RtPrintf("Failed to set servo inputs: 0x%x\n", Code);
   Destroy();
   goto End;
}

啟用熱插拔 (Hot Connect) 並設定虛擬軸的數量

EnableHotConnect允許 在EtherCAT 網路運行時添加新的硬體。SetConfiguredAxesCount 設定模擬軸的數量，在此範例中使用一個模擬軸。

Code = EnableHotConnect(TRUE);
Code = SetConfiguredAxesCount(1);

完成子系統配置

KINGSTAR 提供許多配置子系統之設定，此範例僅使用其中一部分，其他設定可至線上或離線幫助查看 > KINGSTAR RT 與 Win32 API > KINGSTAR 總線 > Functions > 軸變數。

更多設定為選擇性的，但有兩個設定為必設：EtherCAT 循環時間和存取模式，若兩者設定的預設值皆不符所需，請將值更改為更適合的值。

在此範例中使用以下代碼以表示子系統配置已完成：

RtPrintf("Subsystem configured\n");

輸出：

開啟 KINGSTAR 子系統

我們使用 Start 來開啟 KINGSTAR 子系統和 EtherCAT 網路，當子系統無法啟動造成程序永遠在等待完成，我們為 Start 使用 WaitForCommand 將超時設置為 30 秒；若 KINGSTAR 子系統無法啟動，我們使用 Destroy 關閉與子系統的連接並將其終止。

Command = WaitForCommand(30, TRUE, Start());
if (!Command.Done) {
   RtPrintf("Failed to start EtherCAT: 0x%x\n", Command.ErrorId);
   Destroy();
   goto End;
}
RtPrintf("Subsystem Started\n");

輸出：

子系統開啟後，欲擷取其當前狀態，使用 SubsystemStatus 結構及 GetStatus 函式。

SubsystemStatus Subsystem = { ecatOffline, ecatOffline, 
0, 0, 0, {ecatOffline}, {ecatOffline}, {axisOffline} };

SubsystemStatus 包含 EtherCAT 網路的詳細資訊，可從其得知主站和從站的 EtherCAT 狀態、所有 EtherCAT 從站的數量、EtherCAT 網路上的軸與 I/O 模組及其他。我們使用變數 Subsystem 以獲取 EtherCAT 網路的資訊，並分派初始值至 Subsystem，以分辨當資料傳送過去時 Subsystem 的內容是否改變。

Code = GetStatus(&Subsystem, NULL);
if (Code != errNoError) {
   RtPrintf("Failed to get status: 0x%x\n", Code);
   goto Exit;
}
RtPrintf("Subsystem status\n  %d Slaves\n  %d I/Os\n  %d Axes\n\n", 
	Subsystem.SlaveCount, Subsystem.IOCount, Subsystem.AxesCount);

GetStatus獲得所創建的 EtherCAT 網路狀態，此狀態傳送至 Subsystem，因為不使用 NULL，我們將其填入第二參數，接著再使用 Code 以檢查 GetStatus 是否成功執行，若成功，則包含 EtherCAT 從站數量、軸、I/O 模組的子系統狀態將會顯示於 RTX64 伺服器主控台。

輸出：

軸配置

移動軸前須先配置其設定，在配置任何設定前我們宣告以下變數：

int TargetAxis = 0;

TargetAxis 為軸索引。此變數在 #pragma region AxisConfiguration 區塊中使用。

獲取軸狀態

使用 GetAxisByIndex 以獲取軸狀態，將 TargetAxis 設定為零，代表我們欲獲取索引為零的軸資訊。

接著呼叫 GetAxisByIndex，axis 為 SlaveStatus 變數，axisResolution 為我們在 int _tmain 函式開頭所宣告的變數，初始值為 10000，因為不使用最後兩個參數，我們將之設為 NULL。

Code = GetAxisByIndex(TargetAxis, &axis, &axisResolution, NULL, NULL);
if (Code != errNoError) {
   RtPrintf("Failed to get axis status: 0x%x\n", Code);
   goto Exit;
}
RtPrintf("Axis status\n  Name: %s\n  Vendor: 0x%x\n  Product: 0x%x\n  
	Revision: %d\n  Alias: %d\n  Explicit ID: %d\n\n", axis.Name, 
	axis.VendorId, axis.ProductCode, axis.RevisionNumber, 
	axis.AliasAddress, axis.ExplicitId);

我們使用 Code 以檢查函式是否如往常一樣成功執行，若成功，軸零號資訊將會顯示在 RTX64 主控台。

輸出：

轉換軸單位

使用 SetAxisCountsPerUnit 以將軸位置單位轉換為度數，如此可易於知道軸移動了多遠，可使用此函式以設定想要的單位。

Code = SetAxisCountsPerUnit(TargetAxis, axisResolution, 360, FALSE);
if (Code != errNoError) {
   RtPrintf("Failed to set axis unit: 0x%x\n", Code);
   goto Exit;
}

啟用單位轉換

欲套用在 SetAxisCountsPerUnit 中設定的單位，需使用 EnableAxisUnitConversion 以啟用轉換。

Code = EnableAxisUnitConversion(TargetAxis, TRUE);
if (Code != errNoError) {
   RtPrintf("Failed to enable axis unit: 0x%x\n", Code);
   goto Exit;
}

設定軸的控制模式

我們使用 SetAxisControlMode 以設定軸的控制模式，因此範例使用直接位置 (Direct Position) 模式，所以我們將控制模式設為 modeDirectPos，為避免函式執行過長，我們使用 WaitForCommand 以為 SetAxisControlMode 設定 1 秒超時。

Command = WaitForCommand(1, TRUE, SetAxisControlMode(TargetAxis, modeDirectPos));
if (!Command.Done) {
   RtPrintf("Failed to set axis control mode: 0x%x\n", Command.ErrorId);
   goto Exit;
}

更新 PID 與運動參數

單位轉換後必須更新 PID 及運動參數，我們使用 SetAxisVelocityPid 來更新速度模式中的 PID 設定（若軸在扭矩模式則可使用 SetAxisTorquePid）；而 SetAxisMotionProfile 配置軸的運動設定，在 int _tmain 開頭所宣告的實例 Motion 在此套用。

Code = SetAxisVelocityPid(TargetAxis, myPid);
if (Code != errNoError) {
   RtPrintf("Failed to set axis PID: 0x%x\n", Code);
   goto Exit;
}
Code = SetAxisMotionProfile(TargetAxis, profileUnitPerSecond, Motion);
if (Code != errNoError) {
   RtPrintf("Failed to set axis motion profile: 0x%x\n", Code);
   goto Exit;
}

完成軸配置

KINGSTAR 提供多個配置軸的設定，此範例只使用了其中幾個，其他設定資訊可至線上或離線幫助查看 > KINGSTAR RT 與 Win32 API > KINGSTAR Motion > Functions > 軸配置。

大多設定為選擇性的，但控制模式為必設。

在此範例中使用以下代碼以表示軸已配置完成：

RtPrintf("Axis configured\n");

輸出：

每周期檢查軸的操作狀態

我們使用 RegisterCallback 已註冊 CyclicTask 因而讓 KINGSTAR 子系統在每周期有新資料進來時呼叫之，CyclicTask 以恆定速度移動軸。更多關於 CyclicTask 之資訊請見 執行循環任務。

Code = RegisterCallback(&CyclicTask, NULL);

啟動軸

軸完成配置後即可啟動與移動軸。

重設警報

啟動軸之前，我們使用 ResetAxis 以重設軸的警報，以防有任何錯誤，為避免函式執行過長，我們使用 WaitForCommand 以為 ResetAxis 設定 5 秒超時。

Command = WaitForCommand(5, TRUE, ResetAxis(TargetAxis));
if (!Command.Done) {
   RtPrintf("Failed to reset the axis: 0x%x\n", Command.ErrorId);
   goto Exit;
}

啟動軸

使用 PowerAxis 以啟動軸，此函式中的第一個參數為軸索引、第二個參數控制軸的電源，而第三及第四個參數控制軸移動的方向，為避免函式執行過長，我們使用 WaitForCommand 以為 PowerAxis 設定 1 秒超時，

Command = WaitForCommand(1, FALSE, PowerAxis(TargetAxis, TRUE, TRUE, TRUE));
if (!Command.Done) {
   RtPrintf("Failed to enable the axis: 0x%x\n", Command.ErrorId);
   goto Exit;
}
RtPrintf("Axis enabled\n");

軸啟動以後讓軸移動一陣子。

Sleep(30000);

停用軸

軸移動一陣子後，使用 PowerAxis 以停用軸。

Command = WaitForCommand(1, FALSE, PowerAxis(TargetAxis, FALSE, TRUE, TRUE));
if (!Command.Done) {
   RtPrintf("Failed to disable the axis: 0x%x\n", Command.ErrorId);
   goto Exit;
}
RtPrintf("Axis disabled\n");

輸出：

停止 KINGSTAR 子系統

我們使用兩個函式來停止 KINGSTAR 子系統：Stop 和 Destroy。

• Stop停止 EtherCAT 網路與 KINGSTAR 子系統。
• Destroy關閉至 KINGSTAR 子系統的連結並終止該子系統。

因子系統停止需要一些時間，我們使用 WaitForCommand 以提供 10 秒的停止時間，若子系統在 10 秒到後未停止，下一個指令 Destroy 將會執行。

Exit:
//Stop EtherCAT
Command = WaitForCommand(10, FALSE, Stop());
if (!Command.Done) {
   RtPrintf("Failed to stop EtherCAT: %d\n", Command.ErrorId);
}
else {
   RtPrintf("EtherCAT stopped\n");
}
//Stop the Subsystem.
//Never stop the Subsystem if another application or the PLC is running. It would cause a BSOD.
Code = Destroy();
if (Code != errNoError) {
   RtPrintf("Destroy failed: %d\n", Code);
}

Stop和 Destroy 皆包含在 Exit 區塊內，其在子系統未成功啟動時執行，使用這兩個函式時不需要將它們包含在任何區塊內，可以依照需要使用。

End 區塊在函式未成功運行時執行。

End:
RtPrintf("Basic Sample ended\n");

輸出：